#ifndef REMOTE_CONTROL_H
#define REMOTE_CONTROL_H

#include <iostream>
#include "actuatorcontroller.h"
#include "innfos_kinematics.h"
#include <thread>
#include <signal.h>
#include <string.h>
#include <chrono>
#include <math.h>
#include <Eigen/Eigen>
#include <vector>
//#include other .h 

using Vector6=Eigen::Matrix<double,1,6>;
using namespace Eigen;
using namespace std;

class remote_control
{
public:	
	//构造和析构函数	
	remote_control(string ifconfig, int ip, int id_start);
	/**
     * @param ifconfig 网卡名称，可通过ifconfig查询, 如en1o，则ifconfig = "en1o"
     * @param ip ip地址，如ip=1，则实际ip为192.168.1.1
     * @param id_start 起始关节id，如id为91到96,则id_start为91
    **/
	~remote_control(); 
	
	//position and current value
	double pos[6] = {0, 0, 0, 0, 0, 0};
	double cur[6] = {0, 0, 0, 0, 0, 0};
	//gravity compensation value
	double gravity[6] = {0, 0, 0, 0, 0, 0};
	//difference between gravity value and actual current
	double cur_error[6] = {0, 0, 0, 0, 0, 0};
	
	//is remote controller on? If false, on() will print error information, and other public function will do nothing
	bool is_on;

	void on(bool zero_or_not = false);
	/**
     * @brief 使能遥臂关节，并记录初始
     * @param zero_or_not  need to zero or not. default false.
    **/
    
    void zero();
    /**
     * @brief 设置遥臂关节零位
    **/
    
    void manual_zero();
    /**
     * @brief 手动设置遥臂关节零位，运行该程序前需要关节已处于设定的零位
    **/
    
	void off();
	/**
     * @brief 失能遥臂关节，并回到初始位置
    **/
	
	void follow(double * joint);
	/**
     * @brief 将所有遥臂关节设置到位置控制模式，并遥臂每个关节跟随主臂关节运动
     * @param joint 存储主臂关节位置的double数组，弧度
    **/
    
    void follow(Isometry3d & transform3D, double scale=1, double x=0, double y=0, double z=0, double roll=0, double pitch=0, double yaw=0, int rotate_mode=0);
    /**
     * @brief 将所有遥臂关节设置到位置控制模式，并遥臂末端跟随主臂末端运动
     * @param transform3D 存储主臂末端位置
     * @param x 遥臂坐标系映射到从臂坐标系的x轴位移，若遥臂x轴坐标等于0,从臂x轴坐标大于0,则x>0
     * @param y 遥臂坐标系映射到从臂坐标系的y轴位移，若遥臂y轴坐标等于0,从臂y轴坐标大于0,则y>0
     * @param z 遥臂坐标系映射到从臂坐标系的z轴位移，若遥臂z轴坐标等于0,从臂z轴坐标大于0,则z>0
     * @param roll 遥臂坐标系到从臂坐标系的roll角，默认值0，xyz外旋或zyx内旋中x轴对应角度
     * @param pitch 遥臂坐标系到从臂坐标系的pitch角，默认值0，xyz外旋或zyx内旋中y轴对应角度
     * @param yaw 遥臂坐标系到从臂坐标系的yaw角，默认值0，xyz外旋或zyx内旋中z轴对应角度
     * @param scale 从臂比上遥臂的比例，默认值1,若从臂比遥臂大，则scale>1
     * @param rotate_mode 旋转方式，0:xyz外旋，1:zxz内旋，角度顺序按坐标轴顺序
    **/
    
    void follow_fix_startpoint(Isometry3d & transform3D, double scale=1, double roll=0, double pitch=0, double yaw=0, int rotate_mode=0);
    /**
     * @brief 依旧是末端跟随，不同：(1.每次遥臂开始位置固定; 2.五轴角度和从臂位置相反）
     * @param transform3D 存储主臂末端位置
     * @param roll 遥臂坐标系到从臂坐标系的roll角，默认值0，xyz外旋或zyx内旋中x轴对应角度
     * @param pitch 遥臂坐标系到从臂坐标系的pitch角，默认值0，xyz外旋或zyx内旋中y轴对应角度
     * @param yaw 遥臂坐标系到从臂坐标系的yaw角，默认值0，xyz外旋或zyx内旋中z轴对应角度
     * @param scale 从臂比上遥臂的比例，默认值1,若从臂比遥臂大，则scale>1
     * @param rotate_mode 旋转方式，0:xyz外旋，1:zxz内旋，角度顺序按坐标轴顺序
    **/

	void follow_stop();
	/**
     * @brief 遥臂关节停止跟随
    **/
    
	void dragging(double * joint);
	/**
     * @brief 将所有遥臂关节设置到速度控制模式，并开启拖动
     * @param joint 存储遥臂关节位置的double数组，弧度
    **/
    
    void dragging(Isometry3d & transform3D, double scale=1, double x=0, double y=0, double z=0,  double roll=0, double pitch=0, double yaw=0, int rotate_mode=0);
	/**
     * @brief 将所有遥臂关节设置到速度控制模式，并开启拖动
     * @param transform3D 存储遥臂末端位置
     * @param x 遥臂坐标系映射到从臂坐标系的x轴位移，若遥臂x轴坐标等于0,从臂x轴坐标大于0,则x>0
     * @param y 遥臂坐标系映射到从臂坐标系的y轴位移，若遥臂y轴坐标等于0,从臂y轴坐标大于0,则y>0
     * @param z 遥臂坐标系映射到从臂坐标系的z轴位移，若遥臂z轴坐标等于0,从臂z轴坐标大于0,则z>0
     * @param roll 遥臂坐标系到从臂坐标系的roll角，默认值0，xyz外旋或zyx内旋中x轴对应角度
     * @param pitch 遥臂坐标系到从臂坐标系的pitch角，默认值0，xyz外旋或zyx内旋中y轴对应角度
     * @param yaw 遥臂坐标系到从臂坐标系的yaw角，默认值0，xyz外旋或zyx内旋中z轴对应角度
     * @param scale 从臂比上遥臂的比例，默认值1,若从臂比遥臂大，则scale>1
     * @param rotate_mode 旋转方式，0:xyz外旋，1:zxz内旋，角度顺序按坐标轴顺序
    **/
    void dragging_fix_startpoint(Isometry3d & transform3D, double scale=1, double roll=0, double pitch=0, double yaw=0, int rotate_mode=0);
	/**
     * @brief 遥臂末端位置拖动，对应每次固定位置的following
     * @param transform3D 存储遥臂末端位置
     * @param x 遥臂坐标系映射到从臂坐标系的x轴位移，若遥臂x轴坐标等于0,从臂x轴坐标大于0,则x>0
     * @param y 遥臂坐标系映射到从臂坐标系的y轴位移，若遥臂y轴坐标等于0,从臂y轴坐标大于0,则y>0
     * @param z 遥臂坐标系映射到从臂坐标系的z轴位移，若遥臂z轴坐标等于0,从臂z轴坐标大于0,则z>0
     * @param roll 遥臂坐标系到从臂坐标系的roll角，默认值0，xyz外旋或zyx内旋中x轴对应角度
     * @param pitch 遥臂坐标系到从臂坐标系的pitch角，默认值0，xyz外旋或zyx内旋中y轴对应角度
     * @param yaw 遥臂坐标系到从臂坐标系的yaw角，默认值0，xyz外旋或zyx内旋中z轴对应角度
     * @param scale 从臂比上遥臂的比例，默认值1,若从臂比遥臂大，则scale>1
     * @param rotate_mode 旋转方式，0:xyz外旋，1:zxz内旋，角度顺序按坐标轴顺序
    **/
    
    void dragging();
    /**
     * @brief 将所有遥臂关节设置到速度控制模式，并开启拖动
    **/
	
	void dragging_stop();
	/**
     * @brief 遥臂停止拖动
    **/
    
    void adject_sensitivity(double *sensitivity);
    /**
     * @brief 改变遥臂的敏感度
     * @param sensitivity 敏感度的double数组，默认为{100, 0.5, 0.3, 0.15, 0.5, 100} 
    **/
    
    void adject_dragging_speed(double *speed);
    /**
     * @brief 改变遥臂的拖动速度
     * @param speed 拖动速度的double数组，默认为{1, 800, 1200, 2500, 1, 1} 
    **/

protected:
	//protected members here;
	//normally, most of variables and inside functions are here;

	//string of ifconfig order
	char * str2char;
	//id start
	int id;
	//actuator finding times
	int finding_attempt = 0;
	//initial position value, used for avoiding droping off
	double init_pos[6] = {0, 0, 0, 0, 0, 0};
	//position max limit
	double pos_max_limit[6] = {14, 9.5, 14.5, 14.5, 14.5, 36};
	//position min limit
	double pos_min_limit[6] = {-14, -9.5, -14.5, -14.5, -14.5, -36};
	//position mode velocity limit
	double pos_velocity_limit[6] = {500, 250, 500, 500, 500, 500};
	//position mode acceleration limit
	double pos_acceleration_limit[6] = {500, 250, 500, 500, 500, 500};
	//current threshold value
	double curlimit[6] = {100, 0.5, 0.3, 0.15, 0.5, 100};
	//position control ratio
	double posmultiple[6] = {1, 0.5, 0.75, 2, 1, 1};
	//acceleration limit set
	double accellimit[6] = {1200, 500, 1500, 4800, 3600, 1200};
	//velocity limit set
	double vellimit[6] = {1200, 500, 1500, 2400, 1200, 3000};
	//velocity control ratio
	double velratio[6] = {1, 800, 1200, 2500, 1, 1};
	//actuators for current control
	int cur_control[3] = {0, 4, 5};
	//actuators for profile pos control
	int pos_control[] = {};
	//actuators for profile vel control
	int vel_control[3] = {1, 2, 3};
	
	//weight for joints
	double mj = 0.3;
	//weight for arm
	double ma = 0.17;
	//weight for T connector
	double mt = 0.124;
	//arm length
	double l = 0.173;
	//length between joint 4 to 5
	double ls = 0.0792;
	//gravity constant
	double g = 9.81;
	//torque over current
	double torqueratio = 1.8;
	//current filter buffer
	double cur_filter[6][3] = {{0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}};

	//class point
	ActuatorController * pController;
	//uint8_t vector for remote controller's ids
	vector<uint8_t> id_array;
	//actuator array for all joints
	vector<ActuatorController::UnifiedID> uIDArray;
	//actuator array for remote controller
	vector<ActuatorController::UnifiedID> uIDArray_rc;
	
	//innfos_kinematics
	innfos_kinematics ik;
	//ec Define an error type, ec==0x00 means no error, ec will be passed to pcontroller-> lookupActuators(ec) by reference,
    //when the error occurs, ec value will be modified by SDK to the corresponding error code
    Actuator::ErrorsDefine ec;
    
    //array for joint positions of main robotarm
	double * current_joint;
	//pointer to Eigen::Isometry3d of main robotarm's endpoint
	const Isometry3d * current_transform3D;
	//Eigen::Isometry3d of tele robotarm's endpoint
	Isometry3d transform3D_teleoperator;
	//array for joint target positions of main robotarm
	double * target_joint;
	//array for joint position of tele operator
	double joint_teleoperator[6] = {0, 0, 0, 0, 0, 0};
	//pointer to Eigen::Isometry3d of main robotarm's target endpoint
	Isometry3d * target_transform3D;
	//temporary isometry3d used in calculation
	Isometry3d temp_isometry3d;
	
	//rotation direction
	int rotation_direction;
	//rotation and displacement between tele operator and manipulator
	double xyzrpy[6] = {0, 0, 0, 0, 0, 0};
	//
	double startpoint[3] = {0, 0, 0.4};
	//
	double scale_controllee_over_controller;
	
	void poscontrol_on();
	/**
     * @brief 将所有遥臂关节设置到位置控制模式
    **/
    
    void compliance_on();
	/**
     * @brief 将所有遥臂关节设置到速度控制模式（目前部分关节在电流控制模式，待优化）
     * @param joint 存储遥臂关节位置的double数组
    **/
	
	//position control thread related
	pthread_t pth_follow;
	bool follow_bExit = false;
	void poscontrol_loop(); 
	static void * poscontrol_pth(void * __this);
	static void * endpoint_poscontrol_pth(void * __this);
	static void * endpoint_poscontrol_certainposition_pth(void * __this);
	
	//joint compliance control thread related
	pthread_t pth_dragging;
	bool dragging_bExit = false;
	void compliance_loop();
	static void * joint_compliance_control_pth(void * __this);
	static void * endpoint_compliance_control_pth(void * __this);
	static void * endpoint_compliance_certainposition_pth(void * __this);
	
	//forward and inverse kinematic function
	void fk_position();
	void fk_position_certain_position();
    void ik_position();
    void ik_position_certain_position();

private:
	//private members here;
};

#endif
